/*
  Matrix classes copyright © 2004-2005, Jens Lindblad.
*/

#ifndef EXTVEC_INCLUDED
#define EXTVEC_INCLUDED

#include <cmath>
#include <boost/static_assert.hpp>

namespace extvec
{
  const double PI = 3.141592;
  const double PIdiv180 = PI / 180.0;
  
  /*!
   *@brief Policy that does not care if you divide by zero, results will be
   * the default for the system used.
   */
  struct div_zero_policy_ignore {};
  /*!
   *@brief Policy that "fixes" division by zero by replacing the zero by a one,
   * thus doing nothing to change the matrix.
   */
  struct div_zero_policy_fix {};
  
  /*!
   *@brief Matrix.
   *
   *@warning Division_By_Zero_Policy is not used in the code yet. All cases are
   * treated as if div_zero_policy_fix is used.
   */
  template<
    typename T,
    int R,
    int C,
    typename Division_By_Zero_Policy = div_zero_policy_fix
  > class matrix
  {
  public:
    
    BOOST_STATIC_ASSERT(R > 0);
    BOOST_STATIC_ASSERT(C > 0);
    
    typedef T type;
    static const int rows = R;
    static const int cols = C;
    typedef matrix self;
    
    matrix( type v = type(0) )
    {
      for (int i=0; i<rows; ++i)
      {
        for (int j=0; j<cols; ++j)
        {
          _matrix[i][j] = v;
        }
      }
    }
    
    matrix( const type d[rows][cols] )
    {
      for (int i=0; i<rows; ++i)
      {
        for (int j=0; j<cols; ++j)
        {
          _matrix[i][j] = d[i][j];
        }
      }
    }
    
    matrix( type x, type y )
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C == 2);
      _matrix[0][0] = x;
      _matrix[0][1] = y;
    }

    matrix( type x, type y, type z )
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C == 3);
      _matrix[0][0] = x;
      _matrix[0][1] = y;
      _matrix[0][2] = z;
    }
        
    matrix( const self& lhs )
    {
      for (int i=0; i<rows; ++i)
      {
        for (int j=0; j<cols; ++j)
        {
          _matrix[i][j] = lhs(i,j);
        }
      }
    }
    
    ~matrix()
    {
    }
    
    inline type operator()( unsigned long i, unsigned long j ) const
    {
      return _matrix[i][j];
    }
    inline type& operator()( unsigned long i, unsigned long j )
    {
      return _matrix[i][j];
    }
    
    /*!
     *@brief Vector indexing.
     */
    inline type operator()( unsigned long j ) const
    {
      BOOST_STATIC_ASSERT(R == 1);
      return _matrix[0][j];
    }
    inline type& operator()( unsigned long j )
    {
      BOOST_STATIC_ASSERT(R == 1);
      return _matrix[0][j];
    }
    
    self& operator+=( const self& rhs )
    {
      for (int i=0; i<rows; ++i)
      {
        for (int j=0; j<cols; ++j)
        {
          _matrix[i][j] += rhs(i, j);
        }
      }
      return *this;
    }
    
    self& operator-=( const self& rhs )
    {
      for (int i=0; i<rows; ++i)
      {
        for (int j=0; j<cols; ++j)
        {
          _matrix[i][j] -= rhs(i, j);
        }
      }
      return *this;
    }
    
    self& operator*=( type rhs )
    {
      for (int i=0; i<rows; ++i)
      {
        for (int j=0; j<cols; ++j)
        {
          _matrix[i][j] *= rhs;
        }
      }
      return *this;
    }
    
    /*!
     *@brief Division by scalar. Guarded against division by zero.
     */
    self& operator/=( type rhs )
    {
      return this->operator*=(1 / (rhs == 0 ? 1 : rhs));
    }
    
    /*!
     *@brief Left Matrix - Matrix multiplication.
     */
    self& operator*=( const self& rhs )
    {
      BOOST_STATIC_ASSERT(R == C);
      return *this;
    }
    
    /*!
     *@brief Magnitude of vector.
     */
    type magnitude() const
    {
      BOOST_STATIC_ASSERT(R == 1);
      
      type res = type(0);
      for (int j=0; j<cols; ++j)
      {
        res += _matrix[0][j]*_matrix[0][j];
      }
      return std::sqrt(res);
    }
    
    /*!
     *@brief Normalize vector.
     */
    self& normalize()
    {
      BOOST_STATIC_ASSERT(R == 1);
      
      type mag = magnitude();
      return this->operator/=(mag);
    }
    
    type x() const
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C >= 1);
      return _matrix[0][0];
    }
    
    type& x()
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C >= 1);
      return _matrix[0][0];
    }
    
    type y() const
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C >= 2);
      return _matrix[0][1];
    }
    
    type& y()
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C >= 2);
      return _matrix[0][1];
    }
    
    type z() const
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C >= 3);
      return _matrix[0][2];
    }
    
    type& z()
    {
      BOOST_STATIC_ASSERT(R == 1);
      BOOST_STATIC_ASSERT(C >= 3);
      return _matrix[0][2];
    }
    
  private:
    
    type _matrix[rows][cols];
  };
  
  /*!
   *@brief Matrix - Matrix addition.
   */
  template< typename T, int R, int C >
  matrix<T, R, C> operator+( const matrix<T, R, C>& m1, const matrix<T, R, C>& m2 )
  {
    return matrix<T, R, C>(m1) += m2;
  }
  
  /*!
   *@brief Matrix - Matrix subtraction.
   */
  template< typename T, int R, int C >
  matrix<T, R, C> operator-( const matrix<T, R, C>& m1, const matrix<T, R, C>& m2 )
  {
    return matrix<T, R, C>(m1) -= m2;
  }
  
  /*!
   *@brief Matrix - Scalar multiplication.
   */
  template< typename T, int R, int C >
  matrix<T, R, C> operator*( const matrix<T, R, C>& m, typename matrix<T, R, C>::type s )
  {
    return matrix<T, R, C>(m) *= s;
  }

  /*!
   *@brief Scalar - Matrix multiplication.
   */
  template< typename T, int R, int C >
  matrix<T, R, C> operator*( typename matrix<T, R, C>::type s, const matrix<T, R, C>& m )
  {
    return matrix<T, R, C>(m) *= s;
  }
  
  /*!
   *@brief Matrix - Scalar division.
   */
  template< typename T, int R, int C >
  matrix<T, R, C> operator/( const matrix<T, R, C>& m, typename matrix<T, R, C>::type s )
  {
    return matrix<T, R, C>(m) /= s;
  }
  
  template< typename T, int R, int C >
  bool operator==( const matrix<T, R, C>& m1, const matrix<T, R, C>& m2 )
  {
    bool res = true;
    for (int i=0; i<R; ++i)
    {
      for (int j=0; j<C; ++j)
      {
        if (m1(i,j) != m2(i,j))
        {
          res = false;
        }
      }
    }
    return res;
  }
  
  /*!
   *@brief Vector - Vector dot product.
   *
   * Alternatively, accept generic TRC matrices and do a static assert on
   * dimensions. This would be easier for compilers to handle and give better
   * error messages to the user.
   */
  template< typename T, int C >
  typename matrix<T, 1, C>::type dot( const matrix<T, 1, C>& v1, const matrix<T, 1, C>& v2 )
  {
    typename matrix<T, 1, C>::type res(0);
    for (int j=0; j<C; ++j)
    {
      res += v1(j) * v2(j);
    }
    return res;
  }
  
  /*!
   *@brief Vector - Vector cross product. Only for three dimensions right now.
   */
  template< typename T>
  matrix<T, 1, 3> cross( const matrix<T, 1, 3>& v1, const matrix<T, 1, 3>& v2 )
  {	 
    matrix<T, 1, 3> res;
    res = matrix<T, 1, 3>(
      v1.y()*v2.z() - v2.y()*v1.z(),
      v1.z()*v2.x() - v2.z()*v1.x(),
      v1.x()*v2.y() - v2.x()*v1.y());
    return res;
  }
  
  typedef matrix<float, 1, 2> point2f;
  typedef matrix<double, 1, 2> point2d;
  typedef matrix<float, 1, 3> point3f;
  typedef matrix<double, 1, 3> point3d;
  
  typedef matrix<float, 1, 2> vector2f;
  typedef matrix<double, 1, 2> vector2d;
  typedef matrix<float, 1, 3> vector3f;
  typedef matrix<double, 1, 3> vector3d;
  typedef matrix<float, 1, 4> vector4f;
  typedef matrix<double, 1, 4> vector4d;
  typedef matrix<unsigned int, 1, 3> vector3ui;
  
  /*!
   *@brief Simple rectangle class.
   */
  /*template<
    typename T
  > class rect
  {
  public:
    
    rect() {}
    rect( const matrix<T, 1, 2>& __bl,
          const matrix<T, 1, 2>& __size )
      : _bottom_left(__bl), _size(__size) {}
    
    const matrix<T, 1, 2>& bottom_left() const {return _bottom_left;}
    const matrix<T, 1, 2>& size() const {return _size;}
    matrix<T, 1, 2> top_right() const {return _bottom_left + _size;}
    
  private:
    
    matrix<T, 1, 2> _bottom_left;
    matrix<T, 1, 2> _size;
  };*/
  
  //typedef rect<float> rectf;
  
  /*!
   *@brief Check if a given point is inside a rectangle.
   */
  /*template<
    typename T
  > bool inside( const matrix<T, 1, 2>& p, const rect<T>& r )
  {
    return (p.x() >= r.bottom_left().x()) &&
           (p.x() < r.top_right().x()) &&
           (p.y() >= r.bottom_left().y()) &&
           (p.y() < r.top_right().y());
  }*/
}

#endif